#include "DashBoardClient.h"
#include "TcpClient.h"
#include <thread>

namespace CURCONTROL {

DashBoardClient::DashBoardClient():
    ready_(false)
{

}

DashBoardClient::~DashBoardClient()
{

}

bool DashBoardClient::Connect(const std::string &robotIp, std::string &rtnMsg)
{
    bool rtn = false;
    if(BaseClient::Connect(robotIp, DASHBOARD_PORT, false)){
        rtn = true;
    }
    this->StartReceiveThread(
                std::bind(&DashBoardClient::ReceiveCallback, this, std::placeholders::_1));
    return rtn;
}

std::string DashBoardClient::StopProgram()
{
    std::string cmd{"stop"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

std::string DashBoardClient::PlayProgram()
{
    std::string cmd{"play"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

std::string DashBoardClient::PauseProgram()
{
    std::string cmd{"pause"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

std::string DashBoardClient::PowerOn()
{
    std::string cmd{"power on"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

std::string DashBoardClient::PowerOff()
{
    std::string cmd{"power off"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

std::string DashBoardClient::Shutdown()
{
    std::string cmd{"shutdown"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

std::string DashBoardClient::BrakeRelease()
{
    std::string cmd{"brake release"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

std::string DashBoardClient::GetRobotModeStr()
{
    std::string str{"safetymode"};
    RobotMode r = this->GetRobotMode();
    switch (r) {
    case ROBOT_MODE_NO_CONTROLLER:{
        str = "Robotmode: NO_CONTROLLER";
        break;
    }
    case ROBOT_MODE_DISCONNECTED:{
        str = "Robotmode: DISCONNECTED";
        break;
    }
    case ROBOT_MODE_CONFIRM_SAFETY:{
        str = "Robotmode: CONFIRM_SAFETY";
        break;
    }
    case ROBOT_MODE_BOOTING:{
        str = "Robotmode: BOOTING";
        break;
    }
    case ROBOT_MODE_POWER_OFF:{
        str = "Robotmode: POWER_OFF";
        break;
    }
    case ROBOT_MODE_POWER_ON:{
        str = "Robotmode: POWER_ON";
        break;
    }
    case ROBOT_MODE_IDLE:{
        str = "Robotmode: IDLE";
        break;
    }
    case ROBOT_MODE_BACKDRIVE:{
        str = "Robotmode: BACKDRIVE";
        break;
    }
    case ROBOT_MODE_RUNNING:{
        str = "Robotmode: RUNNING";
        break;
    }
    default:
        break;
    }
    return str;
}

RobotMode DashBoardClient::GetRobotMode()
{
    std::string command{"robotmode"};
    command.append("\n");
    if(IsConnected()){
        if(this->tcpClient_->Write(command)){

        }
    }
    std::lock_guard<std::mutex> lk(this->modeMtx_);
    return this->robotmode_;
}

std::string DashBoardClient::GetSafetyModeStr()
{
    std::string str{"safetymode"};
    SafetyMode s = this->GetSafetyMode();
    switch (s) {
    case SAFETY_MODE_NORMAL:{
        str = "Safetymode: NORMAL";
        break;
    }
    case SAFETY_MODE_REDUCED:{
        str = "Safetymode: REDUCED";
        break;
    }
    case SAFETY_MODE_PROTECTIVE_STOP:{
        str = "Safetymode: PROTECTIVE_STOP";
        break;
    }
    case SAFETY_MODE_RECOVERY:{
        str = "Safetymode: RECOVERY";
        break;
    }
    case SAFETY_MODE_SAFEGUARD_STOP:{
        str = "Safetymode: SAFEGUARD_STOP";
        break;
    }
    case SAFETY_MODE_SYSTEM_EMERGENCY_STOP:{
        str = "Safetymode: SYSTEM_EMERGENCY_STOP";
        break;
    }
    case SAFETY_MODE_ROBOT_EMERGENCY_STOP:{
        str = "Safetymode: ROBOT_EMERGENCY_STOP";
        break;
    }
    case SAFETY_MODE_VIOLATION:{
        str = "Safetymode: VIOLATION";
        break;
    }
    case SAFETY_MODE_FAULT:{
        str = "Safetymode: FAULT";
        break;
    }
    default:
        break;
    }
    return str;
}

SafetyMode DashBoardClient::GetSafetyMode()
{
    std::string command{"safetymode"};
    command.append("\n");
    if(IsConnected()){
        if(this->tcpClient_->Write(command)){

        }
    }
    std::lock_guard<std::mutex> lk(this->modeMtx_);
    return this->safetymode_;
}

std::string DashBoardClient::ClosePopup()
{
    std::string cmd{"close popup"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

std::string DashBoardClient::CloseSafetyPopup()
{
    std::string cmd{"close safety popup"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

std::string DashBoardClient::UnlockProtectiveStop()
{
    std::string cmd{"unlock protective stop"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

std::string DashBoardClient::RestartSafety()
{
    std::string cmd{"restart safety"};
    this->DoCommand(cmd);
    std::lock_guard<std::mutex> mk(msgMtx_);
    return dashboardMsg_;
}

void DashBoardClient::Clear()
{
    this->dashboardMsg_.clear();
    this->ready_.store(false);
}

void DashBoardClient::ReceiveCallback(std::vector<uint8_t> &bytes)
{
    std::string str;
    str.assign(bytes.begin(), bytes.end());
    ParseStr(str);
    {
        std::lock_guard<std::mutex> mk(msgMtx_);
        this->dashboardMsg_.assign(bytes.begin(), bytes.end());
    }
    this->ready_.store(true);
    this->conVar_.notify_all();
    bytes.clear();
}

bool DashBoardClient::DoCommand(const std::string &cmd)
{
    std::string command = cmd;
    if(command.back() != '\n'){
        command.append("\n");
    }
    if(IsConnected()){
        if(this->tcpClient_->Write(command)){
//            logger::get(logger::CURCTL_LOG)->info(cmd);
//            printf("DashBoardClient::DoCommand::%s\n", cmd.c_str());
            std::unique_lock<std::mutex> lk(this->conMtx_);
            return this->conVar_.wait_for(lk, std::chrono::seconds(1), [&](){
                if(ready_.load()){
                    this->ready_.store(false);
                    return true;
                }else{
                    return false;
                }
            });
        }else{
            printf("send failed: %s\n", command.c_str());
        }
    }else{
    }
    return false;
}

void DashBoardClient::ParseStr(const std::string &str)
{
    RobotMode r;
    SafetyMode s;
    {
        std::lock_guard<std::mutex> lk(this->modeMtx_);
        r = this->robotmode_;
        s = this->safetymode_;
    }
    if(str.find("Robotmode: NO_CONTROLLER") != std::string::npos){
        r = RobotMode::ROBOT_MODE_NO_CONTROLLER;
    }else if(str.find("Robotmode: DISCONNECTED") != std::string::npos){
        r = RobotMode::ROBOT_MODE_DISCONNECTED;
    }else if(str.find("Robotmode: CONFIRM_SAFETY") != std::string::npos){
        r = RobotMode::ROBOT_MODE_CONFIRM_SAFETY;
    }else if(str.find("Robotmode: BOOTING") != std::string::npos){
        r = RobotMode::ROBOT_MODE_BOOTING;
    }else if(str.find("Robotmode: POWER_OFF") != std::string::npos){
        r = RobotMode::ROBOT_MODE_POWER_OFF;
    }else if(str.find("Robotmode: POWER_ON") != std::string::npos){
        r = RobotMode::ROBOT_MODE_POWER_ON;
    }else if(str.find("Robotmode: IDLE") != std::string::npos){
        r = RobotMode::ROBOT_MODE_IDLE;
    }else if(str.find("Robotmode: BACKDRIVE") != std::string::npos){
        r = RobotMode::ROBOT_MODE_BACKDRIVE;
    }else if(str.find("Robotmode: RUNNING") != std::string::npos){
        r = RobotMode::ROBOT_MODE_RUNNING;
    }else if(str.find("Safetymode: NORMAL") != std::string::npos){
        s = SafetyMode::SAFETY_MODE_NORMAL;
    }else if(str.find("Safetymode: REDUCED") != std::string::npos){
        s = SafetyMode::SAFETY_MODE_REDUCED;
    }else if(str.find("Safetymode: PROTECTIVE_STOP") != std::string::npos){
        s = SafetyMode::SAFETY_MODE_PROTECTIVE_STOP;
    }else if(str.find("Safetymode: RECOVERY") != std::string::npos){
        s = SafetyMode::SAFETY_MODE_RECOVERY;
    }else if(str.find("Safetymode: SAFEGUARD_STOP") != std::string::npos){
        s = SafetyMode::SAFETY_MODE_SAFEGUARD_STOP;
    }else if(str.find("Safetymode: SYSTEM_EMERGENCY_STOP") != std::string::npos){
        s = SafetyMode::SAFETY_MODE_SYSTEM_EMERGENCY_STOP;
    }else if(str.find("Safetymode: ROBOT_EMERGENCY_STOP") != std::string::npos){
        s = SafetyMode::SAFETY_MODE_ROBOT_EMERGENCY_STOP;
    }else if(str.find("Safetymode: VIOLATION") != std::string::npos){
        s = SafetyMode::SAFETY_MODE_VIOLATION;
    }else if(str.find("Safetymode: FAULT") != std::string::npos){
        s = SafetyMode::SAFETY_MODE_FAULT;
    }

    {
        std::lock_guard<std::mutex> lk(this->modeMtx_);
        this->robotmode_ = r;
        this->safetymode_ = s;
    }
}

}// namespace CURCONTROL
